-------------------------------------------------------------------------------
-- RTSAI - Real-Time Strategy engine for AI research
-- Copyright(C) 2006 DIMAp-UFRN, licensed under LGPL
--
-- RTSAI state machine for the seeker in the capture-the-flag example
-------------------------------------------------------------------------------

AVOIDANCE_PREDICT_TIME_MIN = 0.9
AVOIDANCE_PREDICT_TIME_MAX = 2

function clearPathToGoal ( unit )
    local unitPos = unit:position()
    local sideThreshold = unit:radius() * 8.0;
    local behindThreshold = unit:radius() * 2.0;

    local goalOffset = target - unitPos
    local goalDistance = goalOffset:length()
    local goalDirection = goalOffset / goalDistance

    local goalIsAside = unit:isAside ( target )
   
    for i = 0, game.players.attacker:get_num_units() do
        local enemy = game.players.attacker:get_unit(i)
        local eDist = rtsai.Vector3D:distance ( unitPos, enemy:position() )
        local timeEstimate = 0.3 * eDist / enemy:speed()
        local eFuturePosition = enemy:position() + (enemy:velocity() * timeEstimate);
        local eOffset = eFuturePosition - enemy:position()
        local alongCorridor = goalDirection:dot ( eOffset )
        local inCorridor = (alongCorridor > -behindThreshold) and (alongCorridor < goalDistance)
        local eForwardDistance = unitPos:dot ( eOffset )

        if ( inCorridor ) then
            local perp = eOffset - ( goalDirection * alongCorridor )
            local acrossCorridor = perp:length()
            if acrossCorridor < sideThreshold then
                local eFront = eForwardDistance + enemy:radius()
                local eIsBehind = eFront < -behindThreshold
                local eIsWayBehind = eFront < ( -2 * behindThreshold )
                local safeToTurnTowardsGoal = ( eIsBehind and goalIsAside ) or eIsWayBehind

                if not safeToTurnTowardsGoal then
                    return false
                end
            end
        end
    end

    return true
end


function adjustObstacleAvoidanceLookAhead ( unit, clearPath )
    if clearPath then
        evading = false
        local goalDistance = rtsai.Vector3D:distance ( target, unit:position() )
        local headingTowardGoal = unit:isAhead ( target )
        local isNear = ( goalDistance / unit:speed() ) < AVOIDANCE_PREDICT_TIME_MAX
        if headingTowardGoal and not isNear then
            return AVOIDANCE_PREDICT_TIME_MAX
        else
            return AVOIDANCE_PREDICT_TIME_MIN
        end
    else
        evading = true
        return AVOIDANCE_PREDICT_TIME_MIN
    end
end


function steerToEvadeAllDefenders ( unit )
    local evade = rtsai.Vector3D:new (0,0,0)

    for i = 0, game.players.attacker:get_num_units() do
        local enemy = game.players.attacker:get_unit(i)
        local eOffset = enemy:position() - unit:position()
        local eDistance = eOffset:length()

        local timeEstimate = 0.5 * eDistance / enemy:speed()
        local eFuturePosition = enemy:position() + (enemy:velocity() * timeEstimate);

        local flee = unit:steerForFlee ( eFuturePosition )

        local eForwardDistance = (unit:forward()):dot (eOffset)
        local behindThreshold = unit:radius() * -2

        local distanceWeight = 4 / eDistance
        local adjustFlee = flee * distanceWeight
        if eForwardDistance < behindThreshold then
            adjustFlee = adjustFlee / 2
        end

        evade = evade + adjustFlee
    end

    return evade
end

ERROR = 0.001
function isVectorNull ( v )
    return ( math.abs(v.x) < ERROR and math.abs(v.y) < ERROR and math.abs(v.z) < ERROR )
end

State {
    -- STATE ATTRIBUTES -------------------------------------------------------
    name = "seeking_the_flag",

    -- STATE FUNCTIONS --------------------------------------------------------

    act = function ( self )
        --[
        -- simple behavior, just steer to target
        local clearPath = clearPathToGoal ( self.unit )
        log_bool ( "The path is clear", clearPath )

        local avoidancePredictTime = adjustObstacleAvoidanceLookAhead ( self.unit, clearPath )
        log_value ("Predit time for collision", avoidancePredictTime)

        -- drawing for debug
        local futurePos = self.unit:position() + (self.unit:velocity() * avoidancePredictTime);
        local offset = futurePos - self.unit:position()
        local boxSize = rtsai.Vector3D:new ( self.unit:radius(), 0, offset:length() )
        rtsai.OpenSteer.drawBoxOutline ( self.unit, boxSize, rtsai.Vector3D:new ( 0, 0, 1 ) )

        local obst = game:get_objs()
        local obstacleAvoidance = self.unit:steerToAvoidObjects ( avoidancePredictTime, obst )
        log_vector ( "The obstacle avoidance vector is ", obstacleAvoidance )

        if not isVectorNull ( obstacleAvoidance ) then
            -- print "avoiding OBSTACLE"
            action.move.steer = obstacleAvoidance
        else
            if clearPath then
                -- print "steering to TARGET"
                local seek = self.unit:steerForSeek ( target )
                action.move.steer = rtsai.limitMaxDeviationAngle ( seek, 0.707, self.unit:forward() )
            else
                --print "evading all DEFENDERS"
                local evade = steerToEvadeAllDefenders ( self.unit )
                action.move.steer = rtsai.limitMaxDeviationAngle ( evade, 0.707, self.unit:forward() )
            end
        end
        --]]
        --[[
        local seek = self.unit:steerForSeek ( target )
        action.move.steer = seek
        --]]
        return action.move
    end,


    -- EVENT CALLBACKS --------------------------------------------------------

    on_flag_captured = function ( self, event )
        print ("Woohooooo!!!!!  I got it!")
    end,


    on_seeker_captured = function ( self, event )
        print ("Oops!!!")
    end
}
